/*****************************************************************
**                大连理工大学 凌BUG战队
**                **生死有命，富贵在天**
**    没有什么难题是一个通宵解决不了的，如果有那就整两个！
**---------------------------------------------------------------
** 项目名称：   mxdemo_std_robot
** 日    期：   2021-04-15
** 作    者：   MasterWang
**---------------------------------------------------------------
** 文 件 名：   IMU.c
** 文件说明：   惯性测量单元
*****************************************************************/

/*---------------------INCLUDES----------------------*/
#include "IMU.h"
#include "MahonyAHRS.h"
//陀螺仪的姿态角
/*---------------------VARIABLES---------------------*/
//使用CCM数据RAM区(0x10001000-0x10001FFF 4KB)
float IMU_quat[4] __attribute__((at(0x10001000))) = {0.0f};
float IMU_angle[3] __attribute__((at(0x10001010))) = {0.0f}; //euler angle, unit rad.欧拉角 单位 rad
float IMU_angle_deg[3] __attribute__((at(0x10001020))) = {0.0f};

float dial_rotate_angle __attribute__((at(0x10001030))) = 0.0f; //逆时针转刻度盘 0 90 180 270
float IMU_yaw_angle_dial __attribute__((at(0x10001040))) = 0.0f;

/*---------------------FUNCTIONS---------------------*/

/***********************************************************************
** 函 数 名： IMU_init_data()
** 函数说明： 初始IMU变量
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void IMU_init_data(void)
{
	IMU_quat[0] = 1.0f; IMU_quat[1] = 0.0f; IMU_quat[2] = 0.0f; IMU_quat[3] = 0.0f; 
	dial_rotate_angle =0.0f;
}

/***********************************************************************
** 函 数 名： AHRS_update()
** 函数说明： 姿态解算更新(在1KHz外部中断调用)
**---------------------------------------------------------------------
** 输入参数： 三轴陀螺仪数据,三轴加速度数据
** 返回参数： 无
***********************************************************************/
void AHRS_update(float gyro[], float accel[])
{	
	//不使用地磁时调用MahonyAHRSupdateINS
	MahonyAHRSupdateINS(IMU_quat, gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2]);
	
	//
	IMU_angle[0] = atan2f(2.0f*(IMU_quat[0]*IMU_quat[3]+IMU_quat[1]*IMU_quat[2]), 2.0f*(IMU_quat[0]*IMU_quat[0]+IMU_quat[1]*IMU_quat[1])-1.0f);
  IMU_angle[1] = asinf(-2.0f*(IMU_quat[1]*IMU_quat[3]-IMU_quat[0]*IMU_quat[2]));
  IMU_angle[2] = atan2f(2.0f*(IMU_quat[0]*IMU_quat[1]+IMU_quat[2]*IMU_quat[3]),2.0f*(IMU_quat[0]*IMU_quat[0]+IMU_quat[3]*IMU_quat[3])-1.0f);
		
	//
	IMU_angle_deg[0] = IMU_angle[0]*180.0f/3.141592653589f;
	IMU_angle_deg[1] = IMU_angle[1]*180.0f/3.141592653589f;
	IMU_angle_deg[2] = IMU_angle[2]*180.0f/3.141592653589f;
	
	//
	IMU_yaw_dial_update();
}

/***********************************************************************
** 函 数 名： IMU_dial_update()
** 函数说明： 刻度盘算法更新yaw轴角度
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void IMU_yaw_dial_update(void)
{
	//
	IMU_yaw_angle_dial = IMU_angle_deg[IMU_YAW_INDEX] - dial_rotate_angle;
	
	if(IMU_yaw_angle_dial < -180.0f) {IMU_yaw_angle_dial = IMU_yaw_angle_dial + 360.0f;}
	else if(IMU_yaw_angle_dial > 180.0f) {IMU_yaw_angle_dial = IMU_yaw_angle_dial - 360.0f;}
}
